#ifndef _OCTREE_NODE_H
#define _OCTREE_NODE_H

#include "../stdafx.h"
#include <stdio.h>
#include "../debug/print.h"

#include "../objects/entity.h"
#include "../memory/toolkit.h"

#include "octree_manager.h"

#define OCTREE_NODE_SORT_ENTITIES_ADD
#define OCTREE_NODE_SORT_ENTITIES_REMOVE

namespace octree
{

struct entity_mask_info
{
	CEntity * const e;
	unsigned long mask;
//	void operator = (const entity_mask_info & e){}
	inline entity_mask_info(CEntity * const e, const unsigned long mask):e(e), mask(mask){}
};

static unsigned long set_mask = 0xFFFFFFFF;
static unsigned long clr_mask = 0x00000000;
static unsigned long sgn_mask = 0x7FFFFFFF;

static const __m128 g_sgn_mask_vct = _mm_load1_ps(reinterpret_cast<float *>(&sgn_mask));

class COCTreeNode
{
private:
	__m128 center;
	entity_mask_info * entities;
	unsigned long ent_cnt;
	const char * _NAME;

/*
	static const unsigned long entlist_max_cnt = 64;
	static const unsigned long entlist_max_depth = 16;
	static const unsigned long entlist_size = 64;
*/
	static const unsigned long entlist_max_cnt = 2;
	static const unsigned long entlist_max_depth = 128;
	static const unsigned long entlist_size = 64;


protected:
	COCTreeNode * octants; // the 8 octants ... plus header containing parent node's address
public:
	inline COCTreeNode(const float x, const float y, const float z, const float r, const char * _name):center(_mm_set_ps(x, y, z, r)), entities(NULL), octants(NULL), ent_cnt(0), _NAME(_name){}
	inline const __m128 & getCenter() const {return center;}
	inline const unsigned long getEntityCount() const
	{
		if (!octants)
		{
			return ent_cnt;
		}
		return octants[0].getEntityCount() +
			octants[1].getEntityCount() +
			octants[2].getEntityCount() +
			octants[3].getEntityCount() +
			octants[4].getEntityCount() +
			octants[5].getEntityCount() +
			octants[6].getEntityCount() +
			octants[7].getEntityCount();
	}
	/* Return octant at position idx */
	inline COCTreeNode & getOctant(const int idx) const {return octants[idx];}
	/*
	inline unsigned long getParentEntityCount()
	{
		const COCTreeNode * nodes = reinterpret_cast<COCTreeNode *>(reinterpret_cast<unsigned long>(this) & 0xfffff000);
		nodes[1].getEntityCount
	}
*/
	inline void drawNode(void(*drawFunc)(const __m128 & pos), bool draw_octants = true) const
	{
		if (draw_octants && this->octants)
		{
			octants[0].drawNode(drawFunc);
			octants[1].drawNode(drawFunc);
			octants[2].drawNode(drawFunc);
			octants[3].drawNode(drawFunc);
			octants[4].drawNode(drawFunc);
			octants[5].drawNode(drawFunc);
			octants[6].drawNode(drawFunc);
			octants[7].drawNode(drawFunc);
		}
		drawFunc(center);
	}

	/*
	** Return parent's address
	** - assume curent node != root
	*/
	inline COCTreeNode & getParent() const
	{
		const unsigned int octant_addr = reinterpret_cast<unsigned int>(this);
		const unsigned int page_start = octant_addr & 0xfffff000;
		const unsigned int parent_addr = *(reinterpret_cast<unsigned int *>(page_start));
		COCTreeNode * parent = reinterpret_cast<COCTreeNode *>(parent_addr);

		return * parent;
	}

	inline COCTreeNode * getParent(unsigned long & isLast) const
	{
		unsigned int parent_addr = *(reinterpret_cast<unsigned int *>((reinterpret_cast<unsigned int>(this)) & 0xfffff000));
		isLast = parent_addr & 0x00000001;
		return reinterpret_cast<COCTreeNode *>(parent_addr & 0xFFFFFFFE);
	}

	inline const unsigned long getDepth() const
	{
		const unsigned int octant_addr = reinterpret_cast<unsigned int>(this);
		const unsigned int page_start = octant_addr & 0xfffff000;
		const unsigned long depth = *(reinterpret_cast<unsigned long *>(page_start + 4));

		return depth;
	}


	
	inline const char * getNodeName_() const
	{
		#ifdef _DEBUG
		static char name_buff[256];
		unsigned long idx = 0;
		unsigned long dr = 0x0;

		if (!::strcmp(_NAME, "root"))
			dr = 1;

		const COCTreeNode * node = this;

		::strcpy_s(&name_buff[idx], sizeof name_buff, node->_NAME);
		idx += strlen(node->_NAME);
		name_buff[idx++] = ':';

		while (!dr)
		{
			node = node->getParent(dr);
			::strcpy_s(&name_buff[idx], sizeof name_buff, node->_NAME);
			idx += strlen(node->_NAME);
			name_buff[idx++] = ':';
		}
//		::strcpy_s(&name_buff[idx], sizeof name_buff, " **-");
		name_buff[idx - 1] = 0;

		return name_buff;
		#endif // _DEBUG
		return "**";
	}

	/*
	** Perform the memory allocation for the 8 octants
	** - first sizeof(COCNodeTree) reserved for header
	** - header = parent address
	*/
	inline void allocate_octants_()
	{
		octants = toolkit::allocateMemory<COCTreeNode>(9);

		debug::toVsConsole("Allocating octants for %s\n", getNodeName_());

		// store parent's address (see getParent)
		*(reinterpret_cast<unsigned int *>(octants)) = reinterpret_cast<unsigned int>(this);
		*(reinterpret_cast<unsigned int *>(octants) + 1) = this->getDepth() + 1;
		++octants;
	}

	/*
	** Initialize the octants according to current node position
	*/
	inline void populate_octants_() const
	{
		const float t = center.m128_f32[0] / 2.0f;
		new (octants + 0) COCTreeNode(center.m128_f32[3] + t, center.m128_f32[2] + t, center.m128_f32[1] + t, t, "0");
		new (octants + 1) COCTreeNode(center.m128_f32[3] + t, center.m128_f32[2] + t, center.m128_f32[1] - t, t, "1");
		new (octants + 2) COCTreeNode(center.m128_f32[3] + t, center.m128_f32[2] - t, center.m128_f32[1] + t, t, "2");
		new (octants + 3) COCTreeNode(center.m128_f32[3] + t, center.m128_f32[2] - t, center.m128_f32[1] - t, t, "3");
		new (octants + 4) COCTreeNode(center.m128_f32[3] - t, center.m128_f32[2] + t, center.m128_f32[1] + t, t, "4");
		new (octants + 5) COCTreeNode(center.m128_f32[3] - t, center.m128_f32[2] + t, center.m128_f32[1] - t, t, "5");
		new (octants + 6) COCTreeNode(center.m128_f32[3] - t, center.m128_f32[2] - t, center.m128_f32[1] + t, t, "6");
		new (octants + 7) COCTreeNode(center.m128_f32[3] - t, center.m128_f32[2] - t, center.m128_f32[1] - t, t, "7");
	}

	/*
	** Subdivide current node and move contained entities to sub-nodes (octants)
	** - Node becomes a high-level node
	** - The 8 octants are allocated
	*/
	inline void subdivide_()
	{
		assert(!octants);
//		debug::toVsConsole("subdivide_ %s\n", getNodeName_());
		allocate_octants_();
		populate_octants_();
		if (entities)
		{
			registerEntities_(entities, ent_cnt, COCTreeManager::getTmpBuff1(), COCTreeManager::getTmpBuff1(), false, false);
			toolkit::releaseMemory<entity_mask_info>(entities);
			entities = NULL;
			ent_cnt = 0;
		}
		COCTreeManager::removeFromActive(this);
	}

	/*
	** Compact the 8 octants into a single node
	** - Node effectively becomes a leaf
	** - Node conpaction is recursive
	*/
	inline void compact_()
	{
		assert(octants);
		debug::toVsConsole("compact_ %s\n", getNodeName_());

		COCTreeNode * octantsToDelete = octants;
		octants = NULL;

		for (int i = 0; i < 8; i++)
		{
			COCTreeNode & child = octantsToDelete[i];
			if (child.octants)
				child.compact_();
			registerEntitiesInLeafNode_(child.entities, child.ent_cnt);
			child.cleanup_();
		}
		toolkit::releaseMemory<COCTreeNode>(--octantsToDelete);
	}

	/*
	** Remove entity address from node's entityList
	** - Must be a leaf node
	*/
	inline void remove_(CEntity * const e)
	{
		assert(!octants);
//		debug::toVsConsole("remove_ %s, (%.2f, %.2f, %.2f, %.2f)\n", getNodeName_(), e->pos.m128_f32[3], e->pos.m128_f32[2], e->pos.m128_f32[1], e->pos.m128_f32[0]);
		if (ent_cnt)
		{

#ifndef OCTREE_NODE_SORT_ENTITIES_REMOVE

			unsigned long idx = 0;

			while (idx < ent_cnt && e != entities[idx].e)
				++idx;

			// FIXME : check si idx est sur dernier element ...
			if (idx != ent_cnt)
			{
				--ent_cnt;
				if (idx != ent_cnt)
				{
					const entity_mask_info & e = entities[ent_cnt]; // copy the last entity in list to the deleted location
					new (entities + idx) entity_mask_info(e.e, e.mask);
				}
			}
#else
			const unsigned long idx_last = ent_cnt - 1;

			if (entities[idx_last].e == e)
			{
				--ent_cnt;
			}

			else if (e >= entities[0].e && e < entities[idx_last].e)
			{
				for (unsigned long i = 0; i < ent_cnt; i++)
				{
					if (entities[i].e == e)
					{
						::memmove(&entities[i], &entities[i + 1], (idx_last - i) * sizeof * entities);
						--ent_cnt;
					}
				}
			}
#endif
			if (!ent_cnt)
			{
				toolkit::releaseMemory(entities);
				entities = NULL;
				COCTreeManager::removeFromActive(this);
			}

		}
		testIfCompactionPossible_();

	}

	inline void testIfCompactionPossible_()
	{
		unsigned long isLast;
		COCTreeNode * const parent = getParent(isLast);
		if (!isLast && parent->getEntityCount() <= entlist_max_cnt)
			COCTreeManager::addToCompacted(parent);
	}

	inline void testIfSubdivisionPossible_()
	{
		if (ent_cnt >= entlist_max_cnt && getDepth() < entlist_max_depth)
		{
			if (COCTreeManager::allowSubdividedPush)
				COCTreeManager::addToSubdivised(this);
			else
				subdivide_();
		}
	}

	/*
	** Add entity's address to node's entityList
	** - If this node is not a leaf, propagate down to octants
	** - Entities are sorted by address, ascending
	*/
	void add_(CEntity * const e, const unsigned long mask)
	{
//		debug::toVsConsole("add_ %s, (%.2f, %.2f, %.2f, %.2f)\n", getNodeName_(), e->pos.m128_f32[3], e->pos.m128_f32[2], e->pos.m128_f32[1], e->pos.m128_f32[0]);

		if (octants)
		{
			registerEntity_downward_only_(e);
			return;
		}

		if (!entities)
		{
			entities = toolkit::allocateMemory<entity_mask_info>(entlist_size);
			new (entities) entity_mask_info(e, mask);
			ent_cnt = 1;
			COCTreeManager::setActive(this);
			return;
		}

		if (!(ent_cnt % entlist_size))
		{
			entity_mask_info * ents = toolkit::allocateMemory<entity_mask_info>(static_cast<unsigned long>(ent_cnt * 1.618f));
			::memcpy(ents, entities, ent_cnt * sizeof * entities);
			toolkit::releaseMemory<entity_mask_info>(entities);
			entities = ents;
		}

#ifndef OCTREE_NODE_SORT_ENTITIES_ADD

		new (entities + ent_cnt) entity_mask_info(e, mask);
		++ent_cnt;
		testIfSubdivisionPossible_();

#else

		if (entities[ent_cnt - 1].e < e) // Add entity at the end of the list
		{
			new (entities + ent_cnt++) entity_mask_info(e, mask);
			testIfSubdivisionPossible_();
			return;
		}
		
		if (entities[0].e > e) // Add entity at the start of the list
		{
			::memmove(&entities[1], &entities[0], ent_cnt * sizeof * entities);
			new (entities) entity_mask_info(e, mask);
			++ent_cnt;
			testIfSubdivisionPossible_();
			return;
		}

		unsigned long p[2] = {0, ent_cnt};
		unsigned long m = ent_cnt / 2;
		unsigned long dp = ent_cnt;

		while (entities[m].e != e && dp > 1) // We look for the address closest to e in the node's entity list
		{
			dp = p[1] - p[0];
			m = p[0] + (dp / 2);
			p[(entities[m].e > e)] = m;
		}

		if (entities[m].e == e) // Is entity already in list ?
			return;

		const unsigned long st = p[1];
		::memmove(&entities[st + 1], &entities[st], (ent_cnt - st) * sizeof * entities);
		new (entities + st) entity_mask_info(e, mask);
		++ent_cnt;

#endif

		return;
	}

	/*
	** Based on the informations contained in mask, register the entity in the node's octants
	*/
	inline void addInOctants_(const unsigned long swtch, const unsigned long mask, CEntity * const e)
	{
		assert(octants);
		//assert((mask & 0x00000f00) == 0); // check if the entity is completely outside the node
		
		// sgn + oct + exc + ovr

		const unsigned long sgn = mask & 0x00000007;

		switch(swtch)
		{
		case 0:
			getOctant(sgn).add_(e, mask);
			break;
		case 1:
			getOctant(sgn).add_(e, mask);
			getOctant(1 ^ sgn).add_(e, mask);
			break;
		case 2:
			getOctant(sgn).add_(e, mask);
			getOctant(2 ^ sgn).add_(e, mask);
			break;
		case 4:
			getOctant(sgn).add_(e, mask);
			getOctant(4 ^ sgn).add_(e, mask);
			break;
		case 3:
			getOctant(sgn).add_(e, mask);
			getOctant(1 ^ sgn).add_(e, mask);
			getOctant(2 ^ sgn).add_(e, mask);
			getOctant(3 ^ sgn).add_(e, mask);
			break;
		case 5:
			getOctant(sgn).add_(e, mask);
			getOctant(1 ^ sgn).add_(e, mask);
			getOctant(4 ^ sgn).add_(e, mask);
			getOctant(5 ^ sgn).add_(e, mask);
			break;
		case 6:
			getOctant(sgn).add_(e, mask);
			getOctant(2 ^ sgn).add_(e, mask);
			getOctant(4 ^ sgn).add_(e, mask);
			getOctant(6 ^ sgn).add_(e, mask);
			break;
		case 7:
			getOctant(0).add_(e, mask);
			getOctant(1).add_(e, mask);
			getOctant(2).add_(e, mask);
			getOctant(3).add_(e, mask);
			getOctant(4).add_(e, mask);
			getOctant(5).add_(e, mask);
			getOctant(6).add_(e, mask);
			getOctant(7).add_(e, mask);
			break;
		default: // entite completement en dehors du noeud
			assert(0);
			break;
		}
	}

	/*
	** Find the octant(s) containing the entity
	** - only propagate down the nodeTree
	*/
	inline void registerEntity_downward_only_(CEntity * const e)
	{
//		debug::toVsConsole("registerEntity_downward_only_ %s, (%.2f, %.2f, %.2f, %.2f)\n", getNodeName_(), e->pos.m128_f32[3], e->pos.m128_f32[2], e->pos.m128_f32[1], e->pos.m128_f32[0]);
		const __m128 & ent_pos = e->position().getXMM();
		const __m128 c = center;
		const __m128 c_sz = _mm_load1_ps(&c.m128_f32[0]);				// octant's half-length
		const unsigned long mask = createMask_(c, c_sz, ent_pos);
//		const unsigned int sgn_msk = mask & 0x00000007;

		// On reference l'entite dans les octants concernes
		addInOctants_((mask >> 4) & 0x00000007, mask, e);
	}

	/*
	** Add a list of entities in a leaf node, compute masks, but do not propagate entities upward
	** (used for compaction)
	*/
	inline void registerEntitiesInLeafNode_(entity_mask_info * ee, unsigned int ent_count)
	{
		assert(!octants);

		const __m128 c = center;
		const __m128 c_sz = _mm_load1_ps(&c.m128_f32[0]);				// octant's half-length

		for (unsigned long idx = 0; idx < ent_count; idx++)
		{
			CEntity * const e = ee[idx].e;
			const __m128 & ent_pos = e->position().getXMM();
			const unsigned long mask = createMask_(c, c_sz, ent_pos);
			add_(e, mask);
		}
	}

	/*
	** Process a list of entities and find the deepest nodes they belong to.
	** - Allows up/down movements along the NodeTree
	*/
	inline void registerEntities_(entity_mask_info * ee, unsigned int ent_count, entity_mask_info * tmp_buff1, entity_mask_info * tmp_buff2, bool isRootNode = false, bool propagateUpward = true)
	{

		debug::toVsConsole("registerEntities_ %s\n", getNodeName_());
		entity_mask_info * entities_in = tmp_buff1;
		entity_mask_info * entities_out = tmp_buff2;

		entity_mask_info * infos[2] = {entities_in, entities_out};

		const __m128 c = center;
		const __m128 c_sz = _mm_load1_ps(&c.m128_f32[0]);					// demi-longeur de l'octant

		// Extraction des masques
		for (unsigned int idx = 0; idx < ent_count; idx++)
		{
			entity_mask_info & nfo = ee[idx];
			const __m128 & ent_pos = nfo.e->position().getXMM();
			const unsigned long mask = createMask_(c, c_sz, ent_pos);
			new ((infos[(mask & 0x0000ff00) != 0])++) entity_mask_info(nfo.e, mask);
		}

		if (entities_in != infos[0])
			addEntities_(entities_in, infos[0] - entities_in);
		if (propagateUpward && entities_out != infos[1])
		{
			if (!isRootNode)
				notifyParent_(entities_out, infos[1] - entities_out);
			else // Entity out of octree
				return;
//				assert(0);
		}
	}

	/*
	**
	*/
	inline unsigned long createMask_(const __m128 & c, const __m128 & c_sz, const __m128 & ent_pos) const
	{
		const __m128 p_sz = _mm_load1_ps(&ent_pos.m128_f32[0]);			// demi-longeur de l'entite
		const __m128 p = _mm_sub_ps(ent_pos, c);						// coord de l'entite relative au centre de l'octant
		const int sgn = _mm_movemask_ps(p);								// on stocke les signes des coords de l'entite pour les reinjecter en fin de calcul
		const __m128 abs_p = _mm_and_ps(g_sgn_mask_vct, p);				// on force les coords de l'entite dans un espace positif
		const __m128 p1 = _mm_sub_ps(abs_p, p_sz);						// p1 = point 'bas' de la boite englobante
		const int oct = _mm_movemask_ps(p1) << 4;						// definit dans quel(s) octant(s) se trouve l'element
		const int exc = _mm_movemask_ps(_mm_sub_ps(c_sz, p1)) << 8;		// si 0, p1 dans octant, sinon indique ou chercher
		const __m128 p2 = _mm_add_ps(abs_p, p_sz);						// p2 = point 'haut' de la boite englobante
		const int ovr = _mm_movemask_ps(_mm_sub_ps(c_sz, p2)) << 12;	// si 0, p2 dans octant, sinon

		return ((sgn + oct + exc + ovr) & 0x0000eeee) >> 1;
	}

	/*
	** [ Main Octree class entry point ]
	** Process a list of entities and find the deepest nodes they belong to.
	** - Allows up/down movements along the NodeTree
	*/

	template <class ENTITY> void registerEntities(ENTITY * ee, unsigned int ent_count)
	{
		debug::toVsConsole("registerEntities %s\n", getNodeName_());

		COCTreeManager::allowSubdividedPush = false;
		entity_mask_info * entities_in = COCTreeManager::getTmpBuff1();
		entity_mask_info * entities_out = COCTreeManager::getTmpBuff2();

		entity_mask_info * infos[2] = {entities_in, entities_out};

		const __m128 c = center;
		const __m128 c_sz = _mm_load1_ps(&c.m128_f32[0]);					// demi-longeur de l'octant

		// Extraction des masques
		for (unsigned int idx = 0; idx < ent_count; idx++)
		{
			ENTITY * const e = &ee[idx];
			const __m128 & ent_pos = e->position().getXMM();
			const unsigned long mask = createMask_(c, c_sz, ent_pos);
			new ((infos[(mask & 0x0000ff00) != 0])++) entity_mask_info(e, mask);
		}
		if (entities_in != infos[0])
			addEntities_(entities_in, infos[0] - entities_in);
		if (entities_out != infos[1])
			notifyParent_(entities_out, infos[1] - entities_out);
		COCTreeManager::allowSubdividedPush = true;
	}

	/*
	** Process a list of entities and propagate then down to the octants referenced by their entity_mask
	** - masks must not be excusive (eg. refence a space excluded from current node)
	*/
	inline void addEntities_(entity_mask_info * ent_list, unsigned long ent_count)
	{
		debug::toVsConsole("addEntities_ %s\n", getNodeName_());
		for (unsigned int idx = 0; idx < ent_count; idx++)
		{
			const entity_mask_info & nfo = ent_list[idx];
			CEntity * const e = nfo.e;
			const unsigned long mask = nfo.mask;
//			const unsigned long sgn = mask & 0x0000000f;
//			const unsigned long oct = (mask & 0x000000f0) >> 4;

			addInOctants_((mask >> 4) & 0x00000007, mask, e);

			/*
			// On reference l'entite dans les octants concernes
//			switch(mask & 0x000000ff) // on selectionne les masques 'oct' et 'exc'
			switch(oct)
			{
			case 0:
				getOctant(sgn).add_(e, mask);
				break;
			case 1:
			case 2:
			case 4:
				getOctant(sgn).add_(e, mask);
				getOctant(4 ^ sgn).add_(e, mask);
				break;
			case 3:
				getOctant(sgn).add_(e, mask);
				getOctant(1 ^ sgn).add_(e, mask);
				getOctant(2 ^ sgn).add_(e, mask);
				getOctant(3 ^ sgn).add_(e, mask);
				break;
			case 5:
				getOctant(sgn).add_(e, mask);
				getOctant(1 ^ sgn).add_(e, mask);
				getOctant(4 ^ sgn).add_(e, mask);
				getOctant(5 ^ sgn).add_(e, mask);
				break;
			case 6:
				getOctant(sgn).add_(e, mask);
				getOctant(2 ^ sgn).add_(e, mask);
				getOctant(4 ^ sgn).add_(e, mask);
				getOctant(6 ^ sgn).add_(e, mask);
				break;
			case 7:
				getOctant(0).add_(e, mask);
				getOctant(1).add_(e, mask);
				getOctant(2).add_(e, mask);
				getOctant(3).add_(e, mask);
				getOctant(4).add_(e, mask);
				getOctant(5).add_(e, mask);
				getOctant(6).add_(e, mask);
				getOctant(7).add_(e, mask);
				break;
			default: // entite completement en dehors du noeud (exc != 0)
				assert(0);
				break;
			}
			*/
		}
	}

	/*
	** Send a list of entities to parent node
	*/
	void notifyParent_(entity_mask_info * ent_list, unsigned long ent_count);

	/* Release allocated memory and reset member variables */
	inline void cleanup_()
	{
		debug::toVsConsole("cleanup_ %s\n", getNodeName_());
		if (octants) toolkit::releaseMemory<COCTreeNode>(octants);
		if (entities) toolkit::releaseMemory<entity_mask_info>(entities);

		COCTreeManager::removeFromCompacted(this);
		COCTreeManager::removeFromActive(this);
//		COCTreeManager::removeFromColliding(this);

		entities = NULL;
		octants = NULL;
		ent_cnt = 0;
	}
	
	/*
	** Recompute position mask and compare with previous one.
	** If different, erase from node and relocate
	*/
	void update_local_entities_();

	/*
	** Update contained entities' information
	** - Handle entity relocation
	*/
	inline void update()
	{
		debug::toVsConsole("update %s\n", getNodeName_());
		if (octants)
		{
			assert(!entities);
			for (unsigned long i = 0; i < 8; i++)
				octants[i].update();
		}

		if (ent_cnt)
		{
			assert(!octants);
			assert(entities);

			update_local_entities_();
		}
	}

	inline bool collide_(const __m128 & from, const __m128 & from_sz, const __m128 & to) const
	{
		const __m128 p_sz = _mm_load1_ps(&to.m128_f32[0]);		// demi-longeur de l'entite
		const __m128 p = _mm_sub_ps(to, from);					// coord de l'entite relative au centre de l'octant
		const __m128 abs_p = _mm_and_ps(g_sgn_mask_vct, p);		// on force les coords de l'entite dans un espace positif
		const __m128 p1 = _mm_sub_ps(abs_p, p_sz);				// p1 = point 'bas' de la boite englobante
		return (_mm_movemask_ps(_mm_sub_ps(from_sz, p1)) == 0);	// si 0, p1 dans octant, sinon indique ou chercher
	}

	inline void testCollisions()
	{
		// FIXME : correct a wtf behaviour in calling function
		if (!ent_cnt)
			return;
		const unsigned long nb_entities = ent_cnt;
		for (unsigned long idx_collider = 0; idx_collider < nb_entities - 1; idx_collider++)
		{
			const __m128 & collider = entities[idx_collider].e->position().getXMM();
			const __m128 collider_sz = _mm_load1_ps(&collider.m128_f32[0]);
			for (unsigned long idx_target = idx_collider + 1; idx_target < nb_entities; idx_target++)
			{
				const __m128 & target = entities[idx_target].e->position().getXMM();
				if (collide_(collider, collider_sz, target))
					COCTreeManager::addToCollidingList(entities[idx_collider].e, entities[idx_target].e);
			}
		}
	}
/*
	inline void testCollisions2()
	{
		const unsigned long nb_entities = ent_cnt;
		for (unsigned long idx_collider = 0; idx_collider < nb_entities - 1; idx_collider++)
		{
			const __m128 & pos = entities[idx_collider].e->position().getXMM();
			const __m128 & pos_sz = _mm_load1_ps(&collider.m128_f32[0]);
			const __m128i & ipos_inf = _mm_cvttps_epi32(_mm_sub_ps(pos, pos_sz));
			const __m128i & ipos_sup = _mm_cvttps_epi32(_mm_add_ps(pos, pos_sz));
		}
	}
*/
};

class COCTree : public COCTreeNode
{
private:
	inline void allocate_octants_()
	{
		octants = toolkit::allocateMemory<COCTreeNode>(9);

		// store parent's address (see getParent)
		*(reinterpret_cast<unsigned int *>(octants)) = reinterpret_cast<unsigned int>(this) | 0x00000001;
		*(reinterpret_cast<unsigned int *>(octants) + 1) = 0;
		++octants;
	}
	inline void subdivide_()
	{
		assert(!octants);
		allocate_octants_();
		populate_octants_();
	}
public:
	COCTree():COCTreeNode(0.0f, 0.0f, 0.0f, 200.0f, "root"){subdivide_();}

};

} // namespace octree
#endif // _OCTREE_NODE_H